#!/usr/bin/env python
#-*- coding:utf-8 -*-

import random
from queue import PriorityQueue
from planner.planner import Planner
from graph.node_graph import NodeGraph
from planner.astar_planner import AStarPlanner

class PRMPlanner(Planner):
    """
    临时参考：
    https://blog.csdn.net/reasonyuanrobot/article/details/107410479?spm=1001.2101.3001.6650.3&utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-3-107410479-blog-78032283.pc_relevant_paycolumn_v3&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-3-107410479-blog-78032283.pc_relevant_paycolumn_v3&utm_relevant_index=6
    """
    def __init__(self, graph=None, start=None, goal=None):
        super(PRMPlanner, self).__init__(graph, start, goal)

    @property
    def road_map(self):
        if not hasattr(self, "_road_map"):
            self._road_map = NodeGraph
        return self._road_map

    @road_map.setter
    def road_map(self, road_map):
        self._road_map = road_map

    def _sample_vetices(self, nodes_num=100):
        _vertices = dict()
        while len(_vertices) <= nodes_num:
            rd_h = random.randint(0, self.graph.height - 1)
            rd_w = random.randint(0, self.graph.width - 1)
            # print("35 sample pos", rd_h, rd_w)
            if not self.graph.blocked((rd_h, rd_w)):
                if (rd_h, rd_w) not in _vertices:
                    _vertices[(rd_h, rd_w)] = (rd_h, rd_w)
                # print("38 list", _vertices)
        _vertices[self.start] = self.start
        _vertices[self.goal] = self.goal
        return _vertices
    
    def _connet_neighhours(self, vertices, k_neighbours=-1):
        edges = {}
        if k_neighbours == -1: #全连接
            k_neighbours = len(vertices)
        for from_u in vertices:
            _priority_queue = PriorityQueue(len(vertices)) #直接使用len(vertices)，避免k_neighbours导致的队列full阻塞
            for to_v in vertices:
                if from_u == to_v:
                    continue
                dist = self.graph.weight(from_u, to_v)
                _priority_queue.put((dist, to_v))
            got_cnt = 0
            while not _priority_queue.empty() and got_cnt < k_neighbours:
                dist, to_v = _priority_queue.get()
                edges[(from_u, to_v)] = dist
                edges[(to_v, from_u)] = dist
                got_cnt += 1
        return edges

    def _build_graph(self, nodes_num=100, k_neighbours=10):
        self.vertices = self._sample_vetices(nodes_num)
        edges = self._connet_neighhours(self.vertices, k_neighbours)
        self.road_map = NodeGraph()
        self.road_map.load(self.vertices, edges)

    def _solve_heuristic_to_goal(self):
        heurs = {}
        for vertex in self.vertices:
            heurs[(vertex, self.goal)] = self.graph.distance(vertex, self.goal)
        return heurs

    def _solve_vertex_positions(self):
        poses = {}
        for vertex in self.vertices:
            poses[vertex] = vertex
        return poses 

    def _search_path(self):
        heristics = self._solve_heuristic_to_goal()
        # positions = self._solve_vertex_positions()
        self.road_map.heuristics = heristics
        # self.road_map.positions = positions
        planner = AStarPlanner()
        planner.plan(self.road_map, self.start, self.goal)
        self.result = planner.result

    def plan(self, graph=None, start=None, goal=None, nodes_num=100, k_neighbours=-1):
        print("plan in graph {} from {} to {}".format(graph, start, goal))
        if not self.check(graph, start, goal):
            return None
        self._build_graph(nodes_num=nodes_num, k_neighbours=k_neighbours)
        self._search_path()
        return self.result

 
if __name__ == "__main__":
    pass
